#include "stdafx.h"
#include "nBodyOrbitDriver.hpp"

// Don't add to Namespace.h since it's only really needed here
using namespace System::Reflection;
using namespace System::Linq::Expressions;

static nBodyOrbitDriver::nBodyOrbitDriver(void) {

	nBodyOrbitDriver::s_Base_Awake = Utility::GeneratePrivateMethodDelegate<OrbitDriver ^>("Awake");
	nBodyOrbitDriver::s_Base_Start = Utility::GeneratePrivateMethodDelegate<OrbitDriver ^>("Start");
	nBodyOrbitDriver::s_Base_OnDestroy = Utility::GeneratePrivateMethodDelegate<OrbitDriver ^>("OnDestroy");
	nBodyOrbitDriver::s_Base_SOH_Get = Utility::GeneratePrivateFieldGetDelegate<OrbitDriver ^, bool>("\1");
	nBodyOrbitDriver::s_Base_SOH_Set = Utility::GeneratePrivateFieldSetDelegate<OrbitDriver ^, bool>("\1");

}

void nBodyOrbitDriver::Awake(void) {

	Debug_Log(__FUNCSIG__);

	nBodyOrbitDriver::s_Base_Awake->Invoke(this);

}


void nBodyOrbitDriver::Start(void) {

	Debug_Log(__FUNCSIG__);

	nBodyOrbitDriver::s_Base_Start->Invoke(this);

}

void nBodyOrbitDriver::OnDestroy(void) {

	Debug_Log(__FUNCSIG__);

	nBodyOrbitDriver::s_Base_OnDestroy->Invoke(this);

}

void nBodyOrbitDriver::FixedUpdate(void) {

	if (this->QueuedUpdate) {

		return;

	}
	
	this->UpdateOrbit();

}

void nBodyOrbitDriver::UpdateOrbit(void) {

	if (!nBodyOrbitDriver::s_Base_SOH_Get->Invoke(this)) {

		return;

	}

	switch (this->updateMode) {
		case OrbitDriver::UpdateMode::TRACK_Phys:
		case OrbitDriver::UpdateMode::IDLE:

			if (this->rigidbody) {
				
				this->TrackRigidbody(this->referenceBody);
				this->CheckDominantBody(this->transform->position);

			}

			break;

		case OrbitDriver::UpdateMode::UPDATE:

			if (this->vessel) {
				
				this->CheckDominantBody(this->referenceBody->position + this->pos);
			}

			this->updateFromParameters();

			break;

		}

		auto x = this->transform->position;
		auto dx = this->orbit->vel.xzy * (double)TimeWarp::fixedDeltaTime;

		Color PathColour;

		if (this->updateMode == OrbitDriver::UpdateMode::UPDATE) {

			PathColour = Color::cyan;

		} else {

			PathColour = Color::white;

		}

		Debug::DrawRay(x, dx, PathColour);

		if (this->drawOrbit) {

			this->orbit->DrawOrbit();

		}

	}
	
void nBodyOrbitDriver::CheckDominantBody(Vector3d RefPos) {

	auto Body = FlightGlobals::getMainBody(RefPos);

	if (this->referenceBody != Body) {

		if (!FlightGlobals::overrideOrbit) {

			this->RecalculateOrbit(Body);

		}

	}

}

void nBodyOrbitDriver::RecalculateOrbit(CelestialBody ^NewReferenceBody) {

	if (this->frameShift) {
		
		return;

	}

	this->frameShift = true;

	MonoBehaviour::print(String::Concat(gcnew array<object ^> {
			Utility::GetPrivateResourceString(93803),
			this->name,
			Utility::GetPrivateResourceString(21397),
			this->referenceBody->theName,
			Utility::GetPrivateResourceString(93852),
			this->pos.ToString(),
			Utility::GetPrivateResourceString(93867),
			this->vel.ToString(),
			Utility::GetPrivateResourceString(93886),
			this->vel.magnitude,
			Utility::GetPrivateResourceString(93891)
		}));

	this->TrackRigidbody(NewReferenceBody);

	MonoBehaviour::print(String::Concat(gcnew array<object ^> {
			Utility::GetPrivateResourceString(93894),
			this->name,
			Utility::GetPrivateResourceString(21397),
			NewReferenceBody->theName,
			Utility::GetPrivateResourceString(93852),
			this->pos.ToString(),
			Utility::GetPrivateResourceString(93867),
			this->orbit->GetVel().ToString(),
			Utility::GetPrivateResourceString(93886),
			this->vel.magnitude,
			Utility::GetPrivateResourceString(93891)
		}));

	this->orbit->epoch = Planetarium::GetUniversalTime() - (double)TimeWarp::fixedDeltaTime;

	if (this->OnReferenceBodyChange) {
		
		this->OnReferenceBodyChange->Invoke(NewReferenceBody);

	}

	this->Invoke(Utility::GetPrivateResourceString(93941), 1.0f);

}

void nBodyOrbitDriver::SetOrbitMode(OrbitDriver::UpdateMode mode) {

	if (mode == OrbitDriver::UpdateMode::UPDATE) {

		if (this->updateMode != OrbitDriver::UpdateMode::UPDATE) {

			this->orbit->epoch = Planetarium::GetUniversalTime() - (double)TimeWarp::fixedDeltaTime;

		}

	}

	this->updateMode = mode;

}

void nBodyOrbitDriver::TrackRigidbody(CelestialBody ^RefBody) {

	this->localCoM = this->vessel->findLocalCenterOfMass();
	this->pos = (this->transform->position + this->transform->rotation * this->localCoM - RefBody->position).xzy;

	if (this->updateMode == OrbitDriver::UpdateMode::IDLE) {

		this->vel = this->orbit->GetRotFrameVel(this->referenceBody);

	}

	if (this->rigidbody) {

		if (!this->rigidbody->isKinematic) {

			this->vel = this->rigidbody->GetPointVelocity(this->transform->TransformPoint(this->localCoM)) + Krakensbane::GetFrameVelocity();
			this->vel = this->vel.xzy + this->orbit->GetRotFrameVel(this->referenceBody);

		}

	}

	this->vel = this->vel + this->referenceBody->GetFrameVel() - RefBody->GetFrameVel();
	this->orbit->UpdateFromStateVectors(this->pos, this->vel, RefBody, Planetarium::GetUniversalTime());

}

void nBodyOrbitDriver::unlockFrameSwitch(void) {

	this->frameShift = false;

}

// OrbitDriver
void nBodyOrbitDriver::updateFromParameters(void) {

	this->orbit->UpdateFromUT(Planetarium::GetUniversalTime());
	this->pos = this->orbit->pos.xzy;
	this->vel = this->orbit->vel;

	if (double::IsNaN(this->pos.x)) {

		MonoBehaviour::print(String::Concat(gcnew array<object ^> {
				Utility::GetPrivateResourceString(93648),
				this->orbit->ObT,
				Utility::GetPrivateResourceString(93661),
				this->orbit->meanAnomaly,
				Utility::GetPrivateResourceString(93672),
				this->orbit->eccentricAnomaly,
				Utility::GetPrivateResourceString(93683),
				this->orbit->trueAnomaly,
				Utility::GetPrivateResourceString(93694),
				this->orbit->radius,
				Utility::GetPrivateResourceString(93713),
				this->vel.ToString(),
				Utility::GetPrivateResourceString(93726),
				this->orbit->an.ToString(),
				Utility::GetPrivateResourceString(93737),
				this->orbit->period,
				Utility::GetPrivateResourceString(2918)
			}));

		if (this->vessel) {

			MonoBehaviour::print(Utility::GetPrivateResourceString(93756) + this->name);

			this->vessel->Unload();
			UnityEngine::Object::Destroy(this->vessel->gameObject);

		}

	}

	if (!this->reverse) {

		this->CoMoffset = this->vessel ? this->transform->rotation * this->localCoM : Vector3::zero;

		if (this->vessel) {

			this->vessel->SetPosition(this->referenceBody->position + this->pos - this->CoMoffset);

		} else if (this->celestialBody) {

			this->celestialBody->position = this->referenceBody->position + this->pos - this->CoMoffset;

		} else {

			this->transform->position = this->referenceBody->position + this->pos - this->CoMoffset;

		}

	} else {

		this->referenceBody->position = (this->celestialBody ? this->celestialBody->position : this->transform->position)
				- this->pos;
	}

}
